#include "memory.h"
#include <string.h>
#include <malloc.h>
#include "shin_log.h"
#include <pspdebug.h>
#include "config.h"

uint8_t heap_mix[max_mix_block][BufferSize] __attribute__((aligned(64)));
int mix_next[max_mix_block];
int mix_head = 0;

void heap_mix_init() __attribute__((constructor));

void heap_mix_init()
{
	int *addr = &mix_next[0];
	int max = max_mix_block;

asm volatile
(
	".set push\n\t"
	".set noreorder\n\t"
	"move $8,$0\n\t"
	"$loop_start:\n\t"
	"addiu $8,$8,1\n\t"
	"sw $8,0(%0)\n\t"
	"bne $8,%1,$loop_start\n\t"
	"addiu %0,%0,4\n\t"
	"sw %1,0(%0)\n\t"
	".set pop\n"
	:
	:"r"(addr),"r"(max)
	:"$8"
);
}

inline void* shin_mix_alloc()
{
	void *ret;
	if(mix_head == max_mix_block)
	{
		fatal_log("No mix memory!");
		sceKernelSleepThread();
	}
	ret = &heap_mix[mix_head];
	mix_head = mix_next[mix_head];
//	special_log("alloc: %d", i);
	return ret;
}

void* shin_malloc_64(int size)
{
	void *ret;
	int mod_64 = size & 0x3f;
	if(mod_64 != 0) size += 64 - mod_64;
	ret = memalign(64, size);
	if(!ret)
	{
		special_log("No Memory!!!!!!!!\n");
		sceKernelSleepThread();
	}
	return ret;
}

void* shin_malloc (int size)
{
	void *ret;
//	debug_log("malloc: %d", size);
	ret = malloc(size);
//	debug_log("malloc: %p", ret);
	if(!ret)
	{
		special_log("No Memory!!!!!!!!");
		sceKernelSleepThread();
	}
	return ret;
}

void shin_free(void *ptr)
{
//	debug_log("free: %p", ptr);
	int i;
	if(ptr < (void*)heap_mix || ptr >= (void*)heap_mix + sizeof(heap_mix))
	{
		free(ptr);
		return;
	}
	i = (ptr - (void*)heap_mix) / BufferSize;
//	special_log("free: %d", i);
	mix_next[i] = mix_head;
	mix_head = i;
}

static rmff_packet_t packet_heap[max_packets];
rmff_packet_t *packet_head = &packet_heap[0];

void packet_heap_init() __attribute__((constructor));

void packet_heap_init()
{
	int i;
	for(i = 0; i < max_packets - 1; i++)packet_heap[i].heap_internal = &packet_heap[i + 1];
	packet_heap[max_packets - 1].heap_internal = 0;
}

SceUID packet_mutex;

rmff_packet_t* shin_alloc_packet()
{
	rmff_packet_t *ret;
	sceKernelWaitSema(packet_mutex, 1, 0);
	if(!packet_head)
	{
		special_log("max packets exceed");
		sceKernelSleepThread();
	}
	ret = packet_head;
	packet_head = packet_head->heap_internal;
	sceKernelSignalSema(packet_mutex, 1);
	return ret;
}

void shin_free_packet(rmff_packet_t *packet)
{
	sceKernelWaitSema(packet_mutex, 1, 0);
	packet->heap_internal = packet_head;
	packet_head = packet;
	sceKernelSignalSema(packet_mutex, 1);
}

inline void* shin_realloc(void *ptr, unsigned int size)
{
	return realloc(ptr, size);
}

inline void* shin_mallocz(unsigned int size)
{
    void *ptr = shin_malloc(size);
    if (ptr)memset(ptr, 0, size);
    return ptr;
}

inline void shin_freep(void *arg)
{
    void **ptr= (void**)arg;
    shin_free(*ptr);
    *ptr = NULL;
}
